
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_wpnav.h
  * @author     baiyang
  * @date       2022-3-10
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <ahrs/ahrs_view.h>
#include <common/gp_defines.h>
#include <common/time/gp_time.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
#include <common/gp_math/gp_scurve.h>
#include <common/gp_math/gp_spline_curve.h>
#include <mc_position_control/mc_position_control.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum WPNavTerrainSource {
    WPNAV_TERRAIN_UNAVAILABLE,
    WPNAV_TERRAIN_FROM_RANGEFINDER,
    WPNAV_TERRAIN_FROM_TERRAINDATABASE,
};

/** @ 
  * @brief  
  */
typedef struct {
    // flags structure
    struct wpnav_flags {
        uint8_t reached_destination     : 1;    // true if we have reached the destination
        uint8_t fast_waypoint           : 1;    // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
        uint8_t wp_yaw_set              : 1;    // true if yaw target has been set
    } _flags;

    ahrs_view*     _ahrs;
    Position_ctrl* _pos_control;
    Attitude_ctrl* _attitude_control;

    // parameters
    Param_float    _wp_speed_cms;          // default maximum horizontal speed in cm/s during missions
    Param_float    _wp_speed_up_cms;       // default maximum climb rate in cm/s
    Param_float    _wp_speed_down_cms;     // default maximum descent rate in cm/s
    Param_float    _wp_radius_cm;          // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
    Param_float    _wp_accel_cmss;         // horizontal acceleration in cm/s/s during missions
    Param_float    _wp_accel_z_cmss;       // vertical acceleration in cm/s/s during missions
    Param_float    _wp_jerk;               // maximum jerk used to generate scurve trajectories in m/s/s/s
    Param_float    _terrain_margin;        // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin

    float _last_wp_speed_cms;  // last recorded WPNAV_SPEED, used for changing speed in-flight
    float _last_wp_speed_up_cms;  // last recorded WPNAV_SPEED_UP, used for changing speed in-flight
    float _last_wp_speed_down_cms;  // last recorded WPNAV_SPEED_DN, used for changing speed in-flight

    // scurve
    struct scurve _scurve_prev_leg;            // previous scurve trajectory used to blend with current scurve trajectory
    struct scurve _scurve_this_leg;            // current scurve trajectory
    struct scurve _scurve_next_leg;            // next scurve trajectory used to blend with current scurve trajectory
    float _scurve_accel_corner;         // scurve maximum corner acceleration in m/s/s
    float _scurve_jerk;                 // scurve jerk max in m/s/s/s
    float _scurve_snap;                 // scurve snap in m/s/s/s/s

    // spline curves
    struct spline_curve _spline_this_leg;      // spline curve for current segment
    struct spline_curve _spline_next_leg;      // spline curve for next segment

    // the type of this leg
    bool _this_leg_is_spline;           // true if this leg is a spline
    bool _next_leg_is_spline;           // true if the next leg is a spline

    // waypoint controller internal variables
    uint32_t    _wp_last_update;        // time of last update_wpnav call
    float       _wp_desired_speed_xy_cms;   // desired wp speed in cm/sec
    Vector3f_t  _origin;                // starting point of trip to next waypoint in cm from ekf origin
    Vector3f_t  _destination;           // target destination in cm from ekf origin
    float       _track_scalar_dt;       // time compression multiplier to slow the progress along the track
    float       _offset_vel;            // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
    float       _offset_accel;          // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
    bool        _paused;                // flag for pausing waypoint controller

    // terrain following variables
    bool           _terrain_alt;   // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
    bool           _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false)
    Param_int8     _rangefinder_use;       // parameter that specifies if the range finder should be used for terrain following commands
    bool           _rangefinder_healthy;   // true if rangefinder distance is healthy (i.e. between min and maximum)
    float          _rangefinder_alt_cm;    // latest distance from the rangefinder
} mc_wpnav;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void wpnav_ctor(mc_wpnav* wpnav, ahrs_view* ahrs, Attitude_ctrl* attitude_control, Position_ctrl* pos_control);

/// wp_and_spline_init - initialise straight line and spline waypoint controllers
///     speed_cms should be a positive value or left at zero to use the default speed
///     stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero
///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
void wpnav_wp_and_spline_init(mc_wpnav* wpnav, float speed_cms, Vector3f_t stopping_point);
void wpnav_wp_and_spline_init2(mc_wpnav* wpnav);

/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
void wpnav_set_speed_xy(mc_wpnav* wpnav, float speed_cms);

/// set current target climb rate during wp navigation
void wpnav_set_speed_up(mc_wpnav* wpnav, float speed_up_cms);

/// set current target descent rate during wp navigation
void wpnav_set_speed_down(mc_wpnav* wpnav, float speed_down_cms);

/// recalculate path with update speed and/or acceleration limits
void wpnav_update_track_with_speed_accel_limits(mc_wpnav* wpnav);

/// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void wpnav_get_wp_stopping_point_xy(mc_wpnav* wpnav, Vector2f_t* stopping_point);

/// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity
void wpnav_get_wp_stopping_point(mc_wpnav* wpnav, Vector3f_t* stopping_point);

/// set_wp_destination waypoint using location class
///     returns false if conversion from location to vector from ekf origin cannot be calculated
bool wpnav_set_wp_destination_loc(mc_wpnav* wpnav, const Location* destination);

/// set next destination using location class
///     returns false if conversion from location to vector from ekf origin cannot be calculated
bool wpnav_set_wp_destination_next_loc(mc_wpnav* wpnav, const Location* destination);

// get destination as a location. Altitude frame will be above origin or above terrain
// returns false if unable to return a destination (for example if origin has not yet been set)
bool wpnav_get_wp_destination_loc(mc_wpnav* wpnav, Location* destination);

/// set_wp_destination - set destination waypoints using position vectors (distance from ekf origin in cm)
///     terrain_alt should be true if destination.z is an altitude above terrain (false if alt-above-ekf-origin)
///     returns false on failure (likely caused by missing terrain data)
bool wpnav_set_wp_destination(mc_wpnav* wpnav, const Vector3f_t* destination, bool terrain_alt);

/// set next destination using position vector (distance from ekf origin in cm)
///     terrain_alt should be true if destination.z is a desired altitude above terrain
///     provide next_destination
bool wpnav_set_wp_destination_next(mc_wpnav* wpnav, const Vector3f_t* destination, bool terrain_alt);

/// set waypoint destination using NED position vector from ekf origin in meters
bool wpnav_set_wp_destination_NED(mc_wpnav* wpnav, const Vector3f_t* destination_NED);

/// set waypoint destination using NED position vector from ekf origin in meters
bool wpnav_set_wp_destination_next_NED(mc_wpnav* wpnav, const Vector3f_t* destination_NED);

/// shifts the origin and destination horizontally to the current position
///     used to reset the track when taking off without horizontal position control
///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
void wpnav_shift_wp_origin_and_destination_to_current_pos_xy(mc_wpnav* wpnav);

/// shifts the origin and destination horizontally to the achievable stopping point
///     used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min)
///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
void wpnav_shift_wp_origin_and_destination_to_stopping_point_xy(mc_wpnav* wpnav);

/// advance_wp_target_along_track - move target location along track from origin to destination
bool wpnav_advance_wp_target_along_track(mc_wpnav* wpnav, float dt);

/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float wpnav_get_wp_distance_to_destination(mc_wpnav* wpnav);

/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t wpnav_get_wp_bearing_to_destination(mc_wpnav* wpnav);

/// update_wpnav - run the wp controller - should be called at 100hz or higher
bool wpnav_update_wpnav(mc_wpnav* wpnav);

static inline float wpnav_get_wp_radius_cm(mc_wpnav* wpnav) { return wpnav->_wp_radius_cm; }

// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum WPNavTerrainSource wpnav_get_terrain_source(mc_wpnav* wpnav);

// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool wpnav_get_terrain_offset(mc_wpnav* wpnav, float* offset_cm);

// convert location to vector from ekf origin.  terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
//      returns false if conversion failed (likely because terrain data was not available)
bool wpnav_get_vector_NEU(const Location *loc, Vector3f_t *vec, bool *terrain_alt);

/// set_spline_destination waypoint using location class
///     returns false if conversion from location to vector from ekf origin cannot be calculated
///     next_destination should be the next segment's destination
///     next_is_spline should be true if path to next_destination should be a spline
bool wpnav_set_spline_destination_loc(mc_wpnav* wpnav, const Location* destination, const Location* next_destination, bool next_is_spline);

/// set next destination (e.g. the one after the current destination) as a spline segment specified as a location
///     returns false if conversion from location to vector from ekf origin cannot be calculated
///     next_next_destination should be the next segment's destination
bool wpnav_set_spline_destination_next_loc(mc_wpnav* wpnav, const Location* next_destination, const Location* next_next_destination, bool next_next_is_spline);

/// set_spline_destination waypoint using position vector (distance from ekf origin in cm)
///     terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
///     next_destination should be set to the next segment's destination
///     next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
///     next_destination.z  must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too)
bool wpnav_set_spline_destination(mc_wpnav* wpnav, const Vector3f_t* destination, bool terrain_alt, const Vector3f_t* next_destination, bool next_terrain_alt, bool next_is_spline);

/// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin
///     next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
///     next_next_destination should be set to the next segment's destination
///     next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin)
///     next_next_destination.z  must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination should be too)
bool wpnav_set_spline_destination_next(mc_wpnav* wpnav, const Vector3f_t* next_destination, bool next_terrain_alt, const Vector3f_t* next_next_destination, bool next_next_terrain_alt, bool next_next_is_spline);

// return terrain following altitude margin.  vehicle will stop if distance from target altitude is larger than this margin
static inline float wpnav_get_terrain_margin(mc_wpnav* wpnav) { return MAX(wpnav->_terrain_margin, 0.1f); }

// return true if range finder may be used for terrain following
static inline bool wpnav_rangefinder_used(mc_wpnav* wpnav) { return wpnav->_rangefinder_use; }
static inline bool wpnav_rangefinder_used_and_healthy(mc_wpnav* wpnav) { return wpnav->_rangefinder_use && wpnav->_rangefinder_healthy; }

/// set pause or resume during wp navigation
static inline void wpnav_set_pause(mc_wpnav* wpnav) { wpnav->_paused = true; }
static inline void wpnav_set_resume(mc_wpnav* wpnav) { wpnav->_paused = false; }

/// get paused status
static inline bool wpnav_paused(mc_wpnav* wpnav) { return wpnav->_paused; }

/// get default target horizontal velocity during wp navigation
static inline float wpnav_get_default_speed_xy(mc_wpnav* wpnav) { return wpnav->_wp_speed_cms; }

/// get default target climb speed in cm/s during missions
static inline float wpnav_get_default_speed_up(mc_wpnav* wpnav) { return wpnav->_wp_speed_up_cms; }

/// get default target descent rate in cm/s during missions.  Note: always positive
static inline float wpnav_get_default_speed_down(mc_wpnav* wpnav) { return fabsf(wpnav->_wp_speed_down_cms); }

/// get_speed_z - returns target descent speed in cm/s during missions.  Note: always positive
static inline float wpnav_get_accel_z(mc_wpnav* wpnav) { return wpnav->_wp_accel_z_cmss; }

/// get_wp_acceleration - returns acceleration in cm/s/s during missions
static inline float wpnav_get_wp_acceleration(mc_wpnav* wpnav) { return wpnav->_wp_accel_cmss; }

/// get_wp_destination waypoint using position vector
/// x,y are distance from ekf origin in cm
/// z may be cm above ekf origin or terrain (see origin_and_destination_are_terrain_alt method)
static inline const Vector3f_t* wpnav_get_wp_destination(mc_wpnav* wpnav) { return &wpnav->_destination; }

/// get origin using position vector (distance from ekf origin in cm)
static inline const Vector3f_t* wpnav_get_wp_origin(mc_wpnav* wpnav) { return &wpnav->_origin; }

/// true if origin.z and destination.z are alt-above-terrain, false if alt-above-ekf-origin
static inline bool wpnav_origin_and_destination_are_terrain_alt(mc_wpnav* wpnav) { return wpnav->_terrain_alt; }

// returns true if update_wpnav has been run very recently
static inline bool wpnav_is_active(mc_wpnav* wpnav) { return (time_millis() - wpnav->_wp_last_update) < 200; }

/// get desired roll, pitch which should be fed into stabilize controllers
static inline float wpnav_get_roll(mc_wpnav* wpnav) { return posctrl_get_roll_cd(wpnav->_pos_control); }
static inline float wpnav_get_pitch(mc_wpnav* wpnav) { return posctrl_get_pitch_cd(wpnav->_pos_control); }
static inline Vector3f_t wpnav_get_thrust_vector(mc_wpnav* wpnav) { return posctrl_get_thrust_vector(wpnav->_pos_control); }

// get target yaw in centi-degrees
static inline float wpnav_get_yaw(mc_wpnav* wpnav) { return posctrl_get_yaw_cd(wpnav->_pos_control); }

/// return the crosstrack_error - horizontal error of the actual position vs the desired position
static inline float wpnav_crosstrack_error(mc_wpnav* wpnav) { return posctrl_crosstrack_error(wpnav->_pos_control);}

/// reached_destination - true when we have come within RADIUS cm of the waypoint
static inline bool wpnav_reached_wp_destination(mc_wpnav* wpnav) { return wpnav->_flags.reached_destination; }

// reached_wp_destination_xy - true if within RADIUS_CM of waypoint in x/y
static inline bool wpnav_reached_wp_destination_xy(mc_wpnav* wpnav) {
    return wpnav_get_wp_distance_to_destination(wpnav) < wpnav->_wp_radius_cm;
}

// returns object avoidance adjusted destination which is always the same as get_wp_destination
// having this function unifies the AC_WPNav_OA and AC_WPNav interfaces making vehicle code simpler
static inline bool wpnav_get_oa_wp_destination(mc_wpnav* wpnav, Location* destination) { return wpnav_get_wp_destination_loc(wpnav, destination); }

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



